/** ###################################################################
**     Filename  : tilt.C
**     Project   : project_xgate
**     Processor : MC9S12XDT512BMFU
**     Compiler  : CodeWarrior HCS12X C Compiler
**     Date/Time : 5/5/2009, 11:27 PM
**     Contents  :
**         User source code
**
**     (c) Copyright UNIS, a.s. 1997-2008
**     UNIS, a.s.
**     Jundrovska 33
**     624 00 Brno
**     Czech Republic
**     http      : www.processorexpert.com
**     mail      : info@processorexpert.com
** ###################################################################*/

/* MODULE tilt */
#include "arctan.h"
#include "fixed.h"
#include "tilt.h"
#include "MMA7455.h"

//FIXME -- rewrite to output value in fixed point gs and rename module

vector_t normal_vector;
short normal_pitch;
short normal_roll;
short normal_theta;




void tilt_Init(void){
  SetUp_ModeOfSensorIon(ION_G4);
  Ion_SwitchMode(ION_STANDBY);
  StaticCalibration_Ion();
   
  Read_AccelerometerIon();
  MMA7455_getVector(&normal_vector);
  tilt_getTilt(&normal_pitch,&normal_roll,&normal_theta);
  
}

void tilt_getTilt( short *pitch, short *roll, short *theta){
  
  struct accel_vector current_accel, diff;
  short my_pitch;
 short my_roll;
 short my_theta;
  volatile short root;
  
  MMA7455_getVector(&diff);
  
  
  root = Sqrt(diff.Y*diff.Y+diff.Z*diff.Z);
  my_pitch = tilt_angle(diff.X,root);
  root =  Sqrt(diff.X*diff.X+diff.Z*diff.Z);
  my_roll  = tilt_angle(-diff.Y,root);
  root = Sqrt(diff.X*diff.X+diff.Y*diff.Y);
  my_theta = tilt_angle(root,diff.Z);

*pitch =  (my_pitch)- normal_pitch;
*roll = (my_roll) - normal_roll;
*theta = (my_theta) - normal_theta;// < 18000)? my_theta: my_theta-36000;
  
}




/* END tilt */

